#include "../inc/tools.h"
#include <fstream>

bool compareVertByAltitude(CVertexO* _v1, CVertexO* _v2)
{
    return _v1->P()[2] < _v2->P()[2];
}

MeshModel* createNormalsLayer ( MeshDocument& _parent, CMeshO& _mesh )
{
	MeshModel* normalsMesh = _parent.addNewMesh("", QString ("normals") );
	
	for ( int i = 0 ; i < _mesh.vn ; i++ )
	{
		if ( _mesh.vert[i].IsV() )
		{
			vcg::tri::Allocator<CMeshO>::AddVertices( normalsMesh->cm, 2 );
			vcg::tri::Allocator<CMeshO>::AddFaces( normalsMesh->cm, 1 );
			
			normalsMesh->cm.vert[ normalsMesh->cm.vn - 2 ].P() = _mesh.vert[i].P();
			normalsMesh->cm.vert[ normalsMesh->cm.vn - 1 ].P() = _mesh.vert[i].N();
			
			normalsMesh->cm.face[ normalsMesh->cm.fn - 1 ].V(0) = &(normalsMesh->cm.vert[ normalsMesh->cm.vn - 1 ]);
			normalsMesh->cm.face[ normalsMesh->cm.fn - 1 ].V(1) = &(normalsMesh->cm.vert[ normalsMesh->cm.vn - 1 ]);
			normalsMesh->cm.face[ normalsMesh->cm.fn - 1 ].V(2) = &(normalsMesh->cm.vert[ normalsMesh->cm.vn - 2 ]);
		}
	}
	
	/* On renseigne sa bounding box */
	vcg::tri::UpdateBounding<CMeshO>::Box(normalsMesh->cm);
	
	/* Et on lui donne la même matrice de transfo que l'original (pour la trackball) */
	normalsMesh->cm.Tr = _parent.mm()->cm.Tr;
	
	return normalsMesh;
}

float scalarProduct2D ( CVertexO _p1, CVertexO _p2 )
{
	return (_p1.P()[0]*_p2.P()[0] + _p1.P()[1]*_p2.P()[1]);
}


CVertexO diffPoint ( CVertexO _p1, CVertexO _p2 )
{
    CVertexO v;
    v.P()[0] = _p1.P()[0]-_p2.P()[0];
    v.P()[1] = _p1.P()[1]-_p2.P()[1];
    v.P()[2] = _p1.P()[2]-_p2.P()[2];
    return v;
}

void saveCircles( std::vector<Circle> _circles, const QString& _fileName )
{
	std::ofstream file ( _fileName.toStdString().c_str(), std::ios::out | std::ios::trunc);
	
	if( !file )
	{
		std::cout<<"Erreur lors de l'enregistrement dans \""<<_fileName.toStdString()<<"\""<<endl;
		return;
	}
	
	file<<_circles.size()<<std::endl;
	
	for ( unsigned int i = 0 ; i < _circles.size() ; i++ )
	{
		file<<_circles[i].getA()<<" "<<_circles[i].getB()<<" "<<_circles[i].getC()<<" "<<_circles[i].getR()<<std::endl;
	}
	
	file.close();
}

void addCircleToLayer ( CMeshO& _circlesMesh, vcg::Point3f _vector, float _a, float _b, float _c, float _r, int _nbSections)
{
	// On conserve le nombre de vertices du mesh
	int vn = _circlesMesh.vn;
	int cpt=vn;

	vcg::Point3f pt;

	// On déclare trois matrices de transformation
	vcg::Matrix44f rotMatrix;
	vcg::Matrix44f transMatrix;
	vcg::Matrix44f transMatrix2;

	// On ajoute autant de points que de sections
	vcg::tri::Allocator<CMeshO>::AddVertices( _circlesMesh, _nbSections );
	vcg::tri::Allocator<CMeshO>::AddFaces( _circlesMesh, _nbSections );

	// On échantillonne le cercle selon le nombre de sections
	for ( float i = 0 ; i < 2*3.14 ; i+= 2*3.14/_nbSections )
	{
		pt.SetZero();

		// On calcule un vecteur orthogonal à l'axe du cercle
		vcg::Point3f vectTemp(_vector[1], -_vector[0], 0);
		// On lui donne la longeur du rayon
		vectTemp.Normalize();
		vectTemp = vectTemp*_r;
		// On définit une matrice de translation à partir de ce vecteur
		transMatrix2.SetTranslate(vectTemp[0], vectTemp[1], vectTemp[2]);

		// On définit une matrice de rotation autour de l'axe du cercle
		rotMatrix.SetRotateRad(i, _vector);

		// translation vers le centre du cercle
		transMatrix.SetTranslate(_a,_b,_c);

		// On empile les matrices pour obtenir une rotation dont de centre, l'axe et le rayon corrspondent au cercle
		vcg::Matrix44f finalMatrix = transMatrix* rotMatrix * transMatrix2 ;


		// On applique cette transformation à un point situé au centre du cercle
		pt = finalMatrix * pt;

		// On ajoute ce point au maillage
		_circlesMesh.vert[ cpt ].P() = pt;

		// On ajoute un segment entre ce point et le précédent
		if(cpt>vn)
		{
			_circlesMesh.face[cpt-1].V(0) = &(_circlesMesh.vert[cpt]);
			_circlesMesh.face[cpt-1].V(1) = &(_circlesMesh.vert[cpt]);
			_circlesMesh.face[cpt-1].V(2) = &(_circlesMesh.vert[cpt-1]);
		}

		cpt++;

	}
	// Dernier segment permettant de fermer le cercle
	_circlesMesh.face[cpt-1].V(0) = &(_circlesMesh.vert[vn]);
	_circlesMesh.face[cpt-1].V(1) = &(_circlesMesh.vert[vn]);
	_circlesMesh.face[cpt-1].V(2) = &(_circlesMesh.vert[cpt-1]);

	/* On renseigne la bounding box du maillage*/
	vcg::tri::UpdateBounding<CMeshO>::Box(_circlesMesh);

}

void addCircleToLayer ( CMeshO& _circlesMesh, Circle _c , int _nbSections )
{
	// On conserve le nombre de vertices du mesh
	int vn = _circlesMesh.vn;
	int cpt=vn;

	vcg::Point3f pt;

	// On déclare trois matrices de transformation
	vcg::Matrix44f rotMatrix;
	vcg::Matrix44f transMatrix;
	vcg::Matrix44f transMatrix2;

	// On ajoute autant de points que de sections
	vcg::tri::Allocator<CMeshO>::AddVertices( _circlesMesh, _nbSections );
	vcg::tri::Allocator<CMeshO>::AddFaces( _circlesMesh, _nbSections );

	// On échantillonne le cercle selon le nombre de sections
	for ( float i = 0 ; i < 2*3.14 ; i+= 2*3.14/_nbSections )
	{
		pt.SetZero();

		// On calcule un vecteur orthogonal à l'axe du cercle
		vcg::Point3f vectTemp( _c.getNormal()[1], -_c.getNormal()[0], 0);
		// On lui donne la longeur du rayon
		vectTemp.Normalize();
		vectTemp = vectTemp* _c.getR();
		// On définit une matrice de translation à partir de ce vecteur
		transMatrix2.SetTranslate(vectTemp[0], vectTemp[1], vectTemp[2]);

		// On définit une matrice de rotation autour de l'axe du cercle
		rotMatrix.SetRotateRad(i, _c.getNormal());

		// translation vers le centre du cercle
		transMatrix.SetTranslate(_c.getA(),_c.getB(),_c.getC());

		// On empile les matrices pour obtenir une rotation dont de centre, l'axe et le rayon corrspondent au cercle
		vcg::Matrix44f finalMatrix = transMatrix* rotMatrix * transMatrix2 ;


		// On applique cette transformation à un point situé au centre du cercle
		pt = finalMatrix * pt;

		// On ajoute ce point au maillage
		_circlesMesh.vert[ cpt ].P() = pt;
		_circlesMesh.vert[ cpt ].C() =  vcg::Color4b::Red;

		// On ajoute un segment entre ce point et le précédent
		if(cpt>vn)
		{
                        _circlesMesh.face[cpt-1].V(0) = &(_circlesMesh.vert[cpt]);
			_circlesMesh.face[cpt-1].V(1) = &(_circlesMesh.vert[cpt]);
			_circlesMesh.face[cpt-1].V(2) = &(_circlesMesh.vert[cpt-1]);
		}

		cpt++;

	}
	// Dernier segment permettant de fermer le cercle
	_circlesMesh.face[cpt-1].V(0) = &(_circlesMesh.vert[vn]);
	_circlesMesh.face[cpt-1].V(1) = &(_circlesMesh.vert[vn]);
	_circlesMesh.face[cpt-1].V(2) = &(_circlesMesh.vert[cpt-1]);

	/* On renseigne la bounding box du maillage*/
	vcg::tri::UpdateBounding<CMeshO>::Box(_circlesMesh);

}

void addCircleToLayer2 ( CMeshO& _circlesMesh, Circle _c , int _nbSections, float zmin, float zmax, int score )
{
    std::cout << "centre : " << _c.getA() << " - " << _c.getB() << " - " << _c.getC() << std::endl;
    std:: cout << "normale : [" << _c.getNormal().X() << "," << _c.getNormal().Y() << "," << _c.getNormal().Z() << "]" << std::endl ;
    std::cout << "score : " << _c.getScore() << std::endl ;
    // Création d'un cône (d'axe Oy)
    vcg::tri::Cylinder<CMeshO>(30, 10, _circlesMesh) ;
    vcg::Matrix44f homoMatrix ;
    // --- Matrice d'homothétie rayon 1 -> R
    std::cout << "zmax-zmin : " << zmax-zmin << std::endl ;
    homoMatrix.SetScale(vcg::Point3f(_c.getR(), (zmax-zmin)/2, _c.getR())) ;
    for (int i=0; i<_circlesMesh.vn; i++)
    {
        _circlesMesh.vert[i].P() = homoMatrix * _circlesMesh.vert[i].P() ;
        _circlesMesh.vert[i].C().ColorRamp(0, score, _c.getScore()) ;
        _circlesMesh.vert[i].N() = vcg::Point3f(0,0,0) ;
    }
    std::cout << "cone" << std::endl ;
    int vn = _circlesMesh.vn ;
    int fn = _circlesMesh.fn ;
    // Création du centre + normale
    vcg::Point3f P1, P2 ;
//    P1.SetZero();
//    P2.SetZero(); P2.Z()=1 ;

//    vcg::tri::Allocator<CMeshO>::AddVertices(_circlesMesh, 2) ;
//    std::cout << "allocator vertex" << std::endl ;
//    _circlesMesh.vert[vn].P() = P1 ;
//    _circlesMesh.vert[vn].C() = vcg::Color4b::Blue ;
//    _circlesMesh.vert[vn+1].P() = P2 ;
//    _circlesMesh.vert[vn+1].C() = vcg::Color4b::Blue ;
//    vcg::tri::Allocator<CMeshO>::AddFaces(_circlesMesh, 1) ;
//    std::cout << "allocator face" << std::endl ;
//    _circlesMesh.face[fn].V(0) = &(_circlesMesh.vert[vn]);
//    _circlesMesh.face[fn].V(1) = &(_circlesMesh.vert[vn+1]);
//    _circlesMesh.face[fn].V(2) = &(_circlesMesh.vert[vn+1]);
//    std::cout << "face" << std::endl ;
//    vn+=2 ;
//    fn++ ;

    P1.SetZero();
    // Ajout du centre du cercle
    vcg::tri::Allocator<CMeshO>::AddVertices(_circlesMesh, 1) ;
    _circlesMesh.vert[vn].P() = P1 ;
    _circlesMesh.vert[vn].C() = vcg::Color4b::Blue ;
    std::cout << "allocator vertex" << std::endl ;
    vn++ ;

    // Placement du cône
    vcg::Matrix44f rotMatrix, rot2Matrix, transMatrix, finalMatrix;
    vcg::Point3f axis ;
    float angle = 0 ;
    // --- Matrice de rotation Oz -> mainDir
    P2.Import(vcg::Point3f(0,1,0)) ;
    // Cylindre d'axe Oy, donc rotation Oy -> N
    if (P2 != _c.getNormal())
    {
        axis.Import(P2 ^_c.getNormal()) ;
        angle = acos((P2 * _c.getNormal())) ;
        rotMatrix.SetRotateRad(angle, axis) ;
    }
    else
        rotMatrix.SetIdentity();
    std::cout << "Set rotate (angle) : " << angle << std::endl ;
    // --- Matrice de translation O -> centre du cercle
    transMatrix.SetTranslate(_c.getA(), _c.getB(), _c.getC()) ;
    finalMatrix = transMatrix * rotMatrix ;
    // --- Application de la transformation
    for (int i=0 ; i<vn; i++)
    {
        _circlesMesh.vert[i].P() = finalMatrix * _circlesMesh.vert[i].P() ;
    }
    _circlesMesh.vert[vn-1].N() = _c.getNormal() ;
}

MeshModel* createProjLayer (MeshDocument& _parent, MeshModel* _inputMesh, vcg::Point3f _vector)
{
	MeshModel* resultMesh = _parent.addNewMesh("", QString ("projection") );

	// On calcule un vecteur orthogonal à l'axe du cercle
	vcg::Point3f vect1(_vector[1], -_vector[0], 0);

	// Produit vectoriel pour obtenir le troisiéme vecteur
	vcg::Point3f vect2 = vect1 ^ _vector;

	vect1.Normalize();
	vect2.Normalize();

	// On ajoute autant de points que dans le mesh original
	vcg::tri::Allocator<CMeshO>::AddVertices( resultMesh->cm, _inputMesh->cm.VertexNumber());

	for ( int i = 0 ; i < _inputMesh->cm.VertexNumber() ; i++ )
	{
		float X = _inputMesh->cm.vert[i].P() * vect1;
		float Y = _inputMesh->cm.vert[i].P() * vect2;
		vcg::Point3f pt(X, Y, 0);
		resultMesh->cm.vert[ i ].P() = pt;
	}

	/* On renseigne la bounding box du maillage*/
	vcg::tri::UpdateBounding<CMeshO>::Box(resultMesh->cm);

	return resultMesh;

}

void mirror(int &_a, int &_b, int &_c, int &_d, int _maxA, int _maxB, int _maxC, int _maxD)
{
	if ( _a < 0 )
		_a = 0;
	
	if ( _a > _maxA-1 )
		_a = _maxA-1;
	
	if ( _b < 0 )
		_b = 0;
	
	if ( _b > _maxB-1 )
//		_b = _maxB - ( _b - _maxB );
		_b = _maxB-1;
	
	if ( _c < 0 )
		_c = 0;
	
	if ( _c > _maxC-1 )
//		_c = _maxC - ( _c - _maxC );
		_c = _maxC -1;
	
	if ( _d < 0 )
		_d = 0;
	
	if ( _d > _maxD-1 )
//		_d = _maxD - ( _d - _maxD );
		_d = _maxD-1;
}
